#!/usr/bin/env python3

# Software License Agreement (BSD License)
# Copyright © 2014, 2022-2023 belongs to Shadow Robot Company Ltd.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without modification,
# are permitted provided that the following conditions are met:
#   1. Redistributions of source code must retain the above copyright notice,
#      this list of conditions and the following disclaimer.
#   2. Redistributions in binary form must reproduce the above copyright notice,
#      this list of conditions and the following disclaimer in the documentation
#      and/or other materials provided with the distribution.
#   3. Neither the name of Shadow Robot Company Ltd nor the names of its contributors
#      may be used to endorse or promote products derived from this software without
#      specific prior written permission.
#
# This software is provided by Shadow Robot Company Ltd "as is" and any express
# or implied warranties, including, but not limited to, the implied warranties of
# merchantability and fitness for a particular purpose are disclaimed. In no event
# shall the copyright holder be liable for any direct, indirect, incidental, special,
# exemplary, or consequential damages (including, but not limited to, procurement of
# substitute goods or services; loss of use, data, or profits; or business interruption)
# however caused and on any theory of liability, whether in contract, strict liability,
# or tort (including negligence or otherwise) arising in any way out of the use of this
# software, even if advised of the possibility of such damage.


import rospy
from sr_grasp.utils import mk_grasp
from sr_grasp.cfg import GraspPlannerConfig
from dynamic_reconfigure.server import Server
from actionlib import SimpleActionServer
from sr_robot_msgs.msg import PlanGraspAction, PlanGraspResult


class GraspPlanner:

    """
    A ros node to quickly generate moveit_msgs/Grasp grasps for the ShadowHand.
    See README.md for details.
    """

    def __init__(self, ):
        """
        @brief Construct a new GraspPlanner, setting up it's publishers,
        subscribers, etc.
        """
        self.config = None
        self.goal = None

        self._config_server = Server(GraspPlannerConfig, self._config_cb)

        self._action_server = SimpleActionServer(
            '~plan_grasp', PlanGraspAction,
            execute_cb=self._execute_cb, auto_start=False)
        self._action_server.start()

    def _config_cb(self, config, level):
        """Incoming dynamic reconfigure requests, stash config on self."""
        self.config = config
        return config

    def _execute_cb(self, goal):
        """
        Return a hardcoded list of grasps for now.
        """
        self.goal = goal
        res = PlanGraspResult()
        res.grasps = self.get_grasps()
        self._action_server.set_succeeded(result=res)

    @staticmethod
    def get_grasps():
        grasps = []

        # power grasp for vertical objects
        power_grasp = mk_grasp(
            joints={
                'FFJ1': 1.0, 'FFJ2': 1.0, 'FFJ3': 1.4, 'FFJ4': 0.0,
                'MFJ1': 1.0, 'MFJ2': 1.0, 'MFJ3': 1.4, 'MFJ4': 0.0,
                'RFJ1': 1.0, 'RFJ2': 1.0, 'RFJ3': 1.4, 'RFJ4': 0.0,
                'LFJ1': 1.0, 'LFJ2': 1.0, 'LFJ3': 1.4, 'LFJ4': 0.0, 'LFJ5': 0.0,
                'THJ1': 1.0365, 'THJ2': 0.6772, 'THJ3': 0.1849, 'THJ4': 1.2196, 'THJ5': 0.0579,
            },
            pre_joints={'THJ4': 1.2296, 'THJ5': -0.4779},
        )
        power_grasp.grasp_pose.pose.position.x = 0.02
        power_grasp.grasp_pose.pose.position.y = -0.085
        power_grasp.grasp_pose.pose.position.z = 0.333
        # This will make the object vertical if it's a mesh from db. The meshes from the clusterer would
        # need a different orientation, but we opt fro the db mesh to look good
        # as it's quite more obvious when it's wrong
        power_grasp.grasp_pose.pose.orientation.x = 0.0
        power_grasp.grasp_pose.pose.orientation.y = 0.707106781
        power_grasp.grasp_pose.pose.orientation.z = 0.0
        power_grasp.grasp_pose.pose.orientation.w = 0.707106781

        grasps.append(power_grasp)

        # pinch grasp when horizontal
        pinch_grasp = mk_grasp(
            joints={
                'FFJ1': 0.02558, 'FFJ2': 0.35155, 'FFJ3': 1.50130, 'FFJ4': -0.01443,
                'MFJ1': 0.01234, 'MFJ2': 0.43822, 'MFJ3': 1.54195, 'MFJ4': 0.27695,
                'RFJ1': -0.00147, 'RFJ2': -0.00384, 'RFJ3': -0.01043, 'RFJ4': 0.01223,
                'LFJ1': -0.01030, 'LFJ2': 0.03253, 'LFJ3': -0.00054, 'LFJ4': -0.00175, 'LFJ5': 0.01373,
                'THJ1': -0.00082, 'THJ2': 0.0347, 'THJ3': 0.12548, 'THJ4': 1.13063, 'THJ5': 0.48021
            },
            pre_joints={
                'THJ1': 0.01, 'THJ2': 0.008, 'THJ3': 0.125, 'THJ4': 1.22, 'THJ5': -0.2,
                'FFJ3': 0.8, 'MFJ3': 0.8}
        )
        pinch_grasp.grasp_pose.pose.position.x = 0.02
        pinch_grasp.grasp_pose.pose.position.y = -0.085
        pinch_grasp.grasp_pose.pose.position.z = 0.333
        pinch_grasp.grasp_pose.pose.orientation.x = -0.5
        pinch_grasp.grasp_pose.pose.orientation.y = 0.5
        pinch_grasp.grasp_pose.pose.orientation.z = -0.5
        pinch_grasp.grasp_pose.pose.orientation.w = 0.5

        grasps.append(pinch_grasp)

        return grasps


if __name__ == "__main__":
    try:
        rospy.init_node("grasp_planner")
        node = GraspPlanner()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
